#include "aimage.h"
#include <math.h>
#include <stdio.h>
#include <assert.h>
#include <stdlib.h>
#include <string.h>
#include "aimage_ppm.h"

#define LOCK_COUNT 100
//#define DEBUG 1

class CIRSCORE: public ALINECALLBACK
{
	AIMAGE *m_image;

	public:

	int last_x;
	int last_y;
	int lock_count;
	int white_count;
	bool bright_found;
	bool dark_found;
	bool in_image;

	CIRSCORE( AIMAGE *i ) 
	{ 
		m_image = i;
		bright_found = false; 
		dark_found = false; 
		in_image = false;
		lock_count = 0;
		white_count = 0;
	}

	void setpixel( int x, int y, float t, APIXEL &p )
	{
		int width, height;

		m_image->get_dim( width, height );

		if ( x < 0 || y < 0 || x >= width || y >= height )
		{
			return;
		}

		in_image = true;

		if ( 0 && p.r == FULL_INT && p.g == FULL_INT && p.b == FULL_INT )
		{
			white_count++;
			if ( white_count > 10 )
			{
				lock_count = 0;
			}
		}

		if ( p.r == 0 && p.g == 0 && p.b == 0 )
		{
			white_count = 0;
			if ( lock_count == 0 )
			{
				last_x = x;	
				last_y = y;	
			}
			lock_count++;
		}
		else
		{
			if ( lock_count < LOCK_COUNT )
			{
				lock_count = 0;
			}
		}
		//printf("%d %d %d %d %d\n", x, y, lock_count, last_x, last_y );
	}
};

static int my_rand() { 
	int rval = rand() >> 8;
	//printf("RAND %d\n", rval );
	return rval; 
}

void AIMAGE::scorecircle( 
	float x, float y, float xr, float yr,
	AERROR_SAMPLE samples[ NUM_CIRCLE_SAMPLES ] )
{
	int iangle;

	for ( iangle = 0; iangle < NUM_CIRCLE_SAMPLES; ++iangle )
	{
		float angle = ((float) iangle ) / ((float) NUM_CIRCLE_SAMPLES ) * M_PI * 2.0f;
		float x0, y0;
		float x1, y1;
		CIRSCORE callback( this );
		float tar_x, tar_y;

		samples[ iangle ].m_sample_found = false;

		tar_x = cos( angle ) * xr;
		tar_y = sin( angle ) * yr;

		x0 = x;
		y0 = y;
		x1 = x + tar_x * 2.0f;
		y1 = y + tar_y * 2.0f;

		//fprintf(stderr, "%g %g -- %g %g %g %g\n", xr, yr, x0, y0, x1, y1 );

		line_callback( x0, y0, x1, y1, callback );

		if ( callback.lock_count > LOCK_COUNT )
		{
			float dis_x, dis_y, dis, tar_dis, error;

			dis_x = callback.last_x - x; 
			dis_y = callback.last_y - y; 

			dis = sqrtf( dis_x * dis_x + dis_y * dis_y );
			tar_dis = sqrtf( tar_x * tar_x + tar_y * tar_y );

			error = dis - tar_dis;
			error = tar_dis - dis;

			//fprintf(stderr, "Angle %d Error %g\n", iangle, error );

			samples[ iangle ].m_sample_found = true;
			samples[ iangle ].m_error = error;

#define DRAW
#ifdef DRAW
			APIXEL r;
			r.r = FULL_INT;
			r.g = 0;
			r.b = 0;

			//fprintf(stderr, "%d %d\n", callback.last_x, callback.last_y );

			if ( 
				callback.last_x > 30 &&
				callback.last_y > 30 &&
				callback.last_x < m_width - 30 &&
				callback.last_y < m_height - 30 )
			{
			for ( int offs = -3; offs <=3; ++offs ) {
			line( 
				callback.last_x -20, callback.last_y+offs, 
				callback.last_x +20, callback.last_y+offs, 
				r ); 
			line( 
				callback.last_x + offs , callback.last_y - 20, 
				callback.last_x + offs , callback.last_y + 20, 
				r );
			}
			line( x, y, callback.last_x, callback.last_y, r );
			}
#endif 
		}
	}

	for ( iangle = 0; iangle < NUM_CIRCLE_SAMPLES; ++iangle )
	{
		if ( !samples[ iangle ].m_sample_found )
		{
			continue;
		}

		int iangle_m1 = ( iangle + NUM_CIRCLE_SAMPLES - 1 ) % NUM_CIRCLE_SAMPLES;
		int iangle_m2 = ( iangle + NUM_CIRCLE_SAMPLES - 2 ) % NUM_CIRCLE_SAMPLES;
		int iangle_p1 = ( iangle + 1 ) % NUM_CIRCLE_SAMPLES;
		int iangle_p2 = ( iangle + 2 ) % NUM_CIRCLE_SAMPLES;

		if ( 	samples[ iangle_m1 ].m_sample_found && 
				samples[ iangle_m2 ].m_sample_found )
		{
			float m2_to_m1 = fabs( 
					samples[ iangle_m2 ].m_error - 
					samples[ iangle_m1 ].m_error );
			float m1_to_m0 = fabs( 
					samples[ iangle_m1 ].m_error - 
					samples[ iangle ].m_error );
			if ( fabs( m2_to_m1 ) * 10.0f < fabs( m1_to_m0 ) && m1_to_m0 > 5.0f )
			{
				samples[ iangle ].m_sample_found = false;
			}
		}
		if ( 	samples[ iangle_p1 ].m_sample_found && 
				samples[ iangle_p2 ].m_sample_found )
		{
			float p2_to_p1 = fabs( 
					samples[ iangle_p2 ].m_error - 
					samples[ iangle_p1 ].m_error );
			float p1_to_p0 = fabs( 
					samples[ iangle_p1 ].m_error - 
					samples[ iangle ].m_error );
			if ( fabs( p2_to_p1 ) * 10.0f < fabs( p1_to_p0 ) && p1_to_p0 > 5.0f )
			{
				samples[ iangle ].m_sample_found = false;
			}
		}
	}
}

void jiggle(
	AIMAGE *image, 
	float &x, float &y, float &r, 
	AERROR_SAMPLE best_error[ NUM_CIRCLE_SAMPLES ] )
{
	int iangle;
	int select_odds = 1;
	int select_slot = -1;
	AVECTOR points[ 4 ];
	int isub;

	for ( iangle = 0; iangle < NUM_CIRCLE_SAMPLES; ++iangle )
	{
		bool no_holes_found = true;

		for ( isub = 0; isub < 13; ++isub )
		{
			int isub_angle = ( iangle + isub ) % NUM_CIRCLE_SAMPLES;
			if ( !best_error[ isub_angle ].m_sample_found )
			{
				no_holes_found = false;
			}
		}
		if ( !no_holes_found )
		{
			continue;
		}
		if ( ( my_rand() % select_odds ) == 0 )
		{
			select_slot = iangle;
			select_odds++;
		}
	}

	if ( select_slot == -1 )
	{
		//fprintf(stderr, "Fail\n");
		return;
	}

	for ( isub = 0; isub < 4; ++isub )
	{
		iangle = ( select_slot + isub * 4 ) % NUM_CIRCLE_SAMPLES;
		float angle;
		float xp, yp;

		angle = ((float) iangle ) / 
						((float) NUM_CIRCLE_SAMPLES ) * M_PI * 2.0f;
		xp = cos( angle ) * ( r -  best_error[ iangle ].m_error ) + x;
		yp = sin( angle ) * ( r -  best_error[ iangle ].m_error ) + y;

#ifdef DEBUG_JIG
		fprintf(stderr, "%g %g\n", xp, yp );
#endif

		points[ isub ].x = xp;
		points[ isub ].y = yp;
		points[ isub ].z = 0;
		points[ isub ].w = 1;
	}

	AVECTOR v0, v1;

	AVECTOR v0_c0, v0_c1, v1_c0, v1_c1;

	v0_c0 = ( points[ 2 ] - points[ 1 ]);
	v0_c1 = ( points[ 0 ] - points[ 1 ]);
	v1_c0 = ( points[ 3 ] - points[ 2 ]);
	v1_c1 = ( points[ 1 ] - points[ 2 ]);
	v0_c0.normalize();
	v0_c1.normalize();
	v1_c0.normalize();
	v1_c1.normalize();

	v0 = v0_c0 + v0_c1;
	v1 = v1_c0 + v1_c1;

	v0.normalize();
	v1.normalize();

#ifdef DEBUG_JIG
	fprintf( stderr, "%g %g and %g %g\n", v0.x, v0.y, v1.x, v1.y );
#endif

	// My equations are...
	// points[ 1 ] + t * v0 == points[ 2 ] + t * v1;
	// points[ 1 ].x + t * v0.x == points[ 2 ].x + t * v1.x;
	// t ( v0.x - v1.x ) = points[ 2 ].x - points[ 1 ].x
	// t = ( points[ 2 ].x - points[ 1 ].x ) / ( v0.x - v1.x );
	// t = ( points[ 2 ].y - points[ 1 ].y ) / ( v0.y - v1.y );

	float tx, ty;

	tx = ( points[ 2 ].x - points[ 1 ].x ) / ( v0.x - v1.x );
	ty = ( points[ 2 ].y - points[ 1 ].y ) / ( v0.y - v1.y );

#ifdef DEBUG_JIGGLE
	fprintf( stderr, "Tx = %g Ty = %g\n", tx, ty );
#endif

	float t;

	if ( fabs( v0.x - v1.x ) > fabs( v0.y - v1.y ))
	{
		t = tx;
	}
	else
	{
		t = ty;
	}

	r = t;
	x = points[ 1 ].x + v0.x * t;
	y = points[ 1 ].y + v0.y * t;

#ifdef DEBUG_JIGGLE
	fprintf(stderr, "%g %g %g\n", x, y, r );
#endif

#define DEBUG_BIG
#ifdef DEBUG_BIG
	int i;
	APIXEL red( 1.0, 0.0, 0.0 );

	for ( i = 0; i < 4; ++i )
	{
		image->line( 	points[ i ].x - 10.0f, points[ i ].y,
						points[ i ].x + 10.0f, points[ i ].y, red );
		image->line( 	points[ i ].x , points[ i ].y - 10.0f,
						points[ i ].x , points[ i ].y + 10.0f, red );
	}
	image->line( 	points[ 1 ].x , points[ 1 ].y,
					points[ 1 ].x + v0.x * 50.0f, points[ 1 ].y + v0.y * 50.0f,
					red );
	image->line( 	points[ 2 ].x , points[ 2 ].y,
					points[ 2 ].x + v1.x * 50.0f, points[ 2 ].y + v1.y * 50.0f,
					red );

	AIMAGE_PPM_UTILS::save( *image, "stdout", false );

	exit( 0 );
#endif
}

void AIMAGE::circlefind( float &xs, float &ys, float &rs, float &best_error_rv )
{
	AERROR_SAMPLE best_error[ NUM_CIRCLE_SAMPLES ];

	float xr = rs;
	float yr = rs;
	float x = xs;
	float y = ys;

	int iindex;
	int schedule;

	scorecircle( x, y, xr, yr, best_error );
	return;

	for ( schedule = 10; schedule >= 0; --schedule )
	{
		int iscale;
		float scale;

		iscale = 1 << schedule;
		scale = ((float) iscale) * .10f;
		
#ifdef DEBUG
		fprintf(stderr, "Schedule %d iscale %d scale %g\n", schedule, iscale, scale );
#endif

		for ( iindex = 0; iindex < 100; ++iindex )
		{
			int iangle;
			float xt, yt, xrt, yrt;
			AERROR_SAMPLE trial_error[ NUM_CIRCLE_SAMPLES ];
			bool move_watch = false;

			xt = x;
			yt = y;
			xrt = xr;
			yrt = yr;
	
#ifdef DIDNT_WORK
			if ( ( iindex % 20 ) == 0 )
			{
				int iangle_most_out = -1;
				float angle_highest = 0.0f;
				float xp, yp, angle;

				for ( iangle = 0; iangle < NUM_CIRCLE_SAMPLES; ++iangle )
				{
					if ( best_error[ iangle ].m_sample_found &&
						best_error[ iangle ].m_error > angle_highest )
					{
						iangle_most_out = iangle;
						angle_highest = best_error[ iangle ].m_error;
					}
				}
				if ( iangle_most_out == -1 )
				{
					continue;
				}
				xrt += angle_highest / 2.0f;
				yrt = xrt;

				angle = ((float) iangle_most_out ) / 
						((float) NUM_CIRCLE_SAMPLES ) * M_PI * 2.0f;
				xp = cos( angle ) * angle_highest;
				yp = sin( angle ) * angle_highest;
		
				xt -= xp;
				yt -= yp;
				//fprintf(stderr, "ITER %d proposed reshape move iangle %d xp %g yp %g mag %g\n", iindex, iangle_most_out, xp, yp, angle_highest );
				//move_watch = true;
			}
			else if ( ( iindex % 20 ) == 10 )
			{
				int iangle_most_in = -1;
				float angle_lowest = 0.0f;
				float xp, yp, angle;

				for ( iangle = 0; iangle < NUM_CIRCLE_SAMPLES; ++iangle )
				{
					if ( best_error[ iangle ].m_sample_found &&
						best_error[ iangle ].m_error < angle_lowest )
					{
						iangle_most_in = iangle;
						angle_lowest = best_error[ iangle ].m_error;
					}
				}
				if ( iangle_most_in == -1 )
				{
					continue;
				}
				xrt -= angle_lowest;
				yrt = xrt;

				angle = ((float) iangle_most_in ) / 
						((float) NUM_CIRCLE_SAMPLES ) * M_PI * 2.0f;
				xp = cos( angle ) * angle_lowest;
				yp = sin( angle ) * angle_lowest;
		
				xt += xp * .5;
				yt += yp * .5;
				fprintf(stderr, "ITER %d proposed reshape move iangle %d xp %g yp %g mag %g\n", iindex, iangle_most_in, xp, yp, angle_lowest );
				// move_watch = true;
			}
#endif
			if ( ( iindex % 10 ) == 0 && schedule < 6 )
			{
				jiggle( this, xt, yt, xrt, best_error );
//				yrt = xrt;
				xrt = yrt;
			}
			else
			{
				xt += (((float) ( my_rand() % 3 )) - 1.0f ) * scale;
				yt += (((float) ( my_rand() % 3 )) - 1.0f ) * scale;
//				xrt += (((float) ( my_rand() % 3 )) - 1.0f ) * scale;
//				yrt = xrt;
			}

			scorecircle( xt, yt, xrt, yrt, trial_error );

			float best_error_score = 0.0f;
			float trial_error_score = 0.0f;

			for ( iangle = 0; iangle < NUM_CIRCLE_SAMPLES; ++iangle )
			{
				if ( 	best_error[ iangle ].m_sample_found &&
						trial_error[ iangle ].m_sample_found )
				{
					best_error_score += fabs( best_error[ iangle ].m_error );
					trial_error_score += fabs( trial_error[ iangle ].m_error );
				}
			}
			if ( schedule == 0 )
			{
				//move_watch = 1;
			}
			if ( move_watch )
			{
				for ( iangle = 0; iangle < NUM_CIRCLE_SAMPLES; ++iangle )
				{
					fprintf(stderr,"   %d %d %d %g %g\n", iangle, 
							best_error[ iangle ].m_sample_found,
							trial_error[ iangle ].m_sample_found,
							best_error[ iangle ].m_error,
							trial_error[ iangle ].m_error );
				}
			}
			if ( best_error_score > trial_error_score )
			{
				for ( iangle = 0; iangle < NUM_CIRCLE_SAMPLES; ++iangle )
				{
					best_error[ iangle ] = trial_error[ iangle ];
				}
				x = xt;
				y = yt;
				xr = xrt;
				yr = yrt;

				best_error_rv = trial_error_score;

#ifdef DEBUG
				fprintf(stderr, "ITER %d %g %g %g %g new error %g old %g\n",
						iindex, 
						x, y, xr, yr, trial_error_score, best_error_score );
#endif
			}
		}
	}

	int iangle;

	for ( iangle = 0; iangle < NUM_CIRCLE_SAMPLES; ++iangle )
	{
		float xp,yp;

		float angle = ((float) iangle ) / ((float) NUM_CIRCLE_SAMPLES ) * M_PI * 2.0f;
		xp = cos( angle ) * xr + x;
		yp = sin( angle ) * yr + y;

		APIXEL r;
		r.r = FULL_INT;
		r.g = 0;
		r.b = 0;

		if ( xp > 10 && yp > 10 &&
			xp < m_width - 10 &&
			yp < m_height - 10 )
		{
				line( xp-10, yp, xp+10, yp, r );
				line( xp, yp-10, xp, yp+10, r ); 
		}
	}

	xs = x;
	ys = y;
	rs = xr;
}

#define MAX( a, b ) ( a > b ? a : b )
#define MAX4( a, b, c, d ) ( MAX( MAX( MAX( a, b ), c ), d ) )

void AIMAGE::mask_for_sunspots( 
	const AIMAGE &src_image, 
	float cutoff, 
	int edge_span )
{
	int x0, y0, x1, y1;
	int icutoff;

	assert( m_height == src_image.m_height );
	assert( m_width == src_image.m_width );

	icutoff = (int) ( cutoff * 256.0f );
	x0 = edge_span;
	y0 = edge_span;
	x1 = m_width - edge_span;
	y1 = m_height - edge_span;

	int y;
	for ( y = y0 ; y < y1; ++y ) 
	{
		int x;
		for ( x = x0 ; x < x1; ++x ) 
		{
			APIXEL top, left, right, bot, cen;
			APIXEL sample;
			int i_sample, i_center;
			top = src_image.m_image[ y -edge_span ][ x ];
			bot = src_image.m_image[ y +edge_span ][ x ];
			left = src_image.m_image[ y ][ x - edge_span ];
			right = src_image.m_image[ y ][ x + edge_span ];
			cen = src_image.m_image[ y ][ x ];

			sample.r = MAX4( top.r, bot.r, left.r, right.r );
			sample.g = MAX4( top.g, bot.g, left.g, right.g );
			sample.b = MAX4( top.b, bot.b, left.b, right.b );

			i_sample = sample.r * 3 + sample.g * 6 + sample.b;
			i_center= cen.r * 3 + cen.g * 6 + cen.b;

			APIXEL &cur = m_image[ y ][ x ];

			if ( i_center * 256 < i_sample * icutoff)
			{
				cur.r = FULL_INT;
				cur.g = FULL_INT;
				cur.b = FULL_INT;
			}
			else
			{
				cur.r = 0;
				cur.g = 0;
				cur.b = 0;
			}
		}
	}
}

void SOLAR_FRAMES_ALIGNMENT::read_solar_align_data( const char *filename )
{
	FILE *f;
	char buffer[ BUFSIZE ];

	f = fopen(filename, "rt" );
	assert( f );
	while ( fgets( buffer, BUFSIZE, f ) != NULL )
	{
		SOLAR_FRAME_ALIGNMENT frame;

		sscanf(buffer, "%s X %g Y %g R %g",
				frame.filename,
				& (frame.center_x),
				& (frame.center_y),
				& (frame.radius)
		);

		frames.push_back( frame );
	}
	fclose( f );
}

void SOLAR_FRAMES_ALIGNMENT::lookup_alignment( const char *image, float &x, float &y, float &r )
{
	vector<SOLAR_FRAME_ALIGNMENT>::iterator cur;
	vector<SOLAR_FRAME_ALIGNMENT>::iterator end;

	for ( cur = frames.begin(), end = frames.end(); cur != end; ++cur )
	{
		if ( strcmp( cur->filename, image ) == 0 )
		{
			x = cur->center_x;
			y = cur->center_y;
			r = cur->radius;
			return;
		}
	}
	assert( 0 );
}

